#pragma once
#include <list>
#include <shared_mutex>

namespace axis {

struct TrajPoint
{
    double pos;
    double vel;
    double acc;
    double jerk;
    double err;  // 预测点的误差，真实规划进来后再更新
    bool is_real;
    bool is_planned;  // 是否已经规划完毕
};

class Planner
{
public:
    enum InterpType
    {
        INTERP_LINEAR,
        INTERP_POLY3,
        INTERP_POLY5,
        INTERP_POLY7,
    };
    enum PredictType
    {
        PREDICT_HOLD_VEL,
        PREDICT_HOLD_ACC,
        PREDICT_HOLD_JERK,
    };

public:
    virtual ~Planner() {}
    virtual void init_traj_planner(double planner_interval, double traj_interval);
    void reset();
    void push_pos(TrajPoint& pos, bool is_jump = false);
    virtual int do_interp(InterpType interp_type, PredictType predict_type);
    virtual int calc_s(double ts, double& pos);

    void get_poly_para(double* para) const;
    virtual int do_plan(InterpType interp_type, double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt);
    virtual TrajPoint do_predict(PredictType predict_type, const TrajPoint& point_start);

    double get_planner_interval() const { return planner_interval_; }
    double get_traj_interval() const { return traj_interval_; }
    double get_tarj_start_flag() const { return is_traj_start; }
    TrajPoint get_planner_target();
    TrajPoint get_planner_pre_target();
    int get_traj_lost_count();

private:
    virtual TrajPoint calc_predict_point(double pos, double vel, double acc, double jerk);
    virtual int do_plan_linear(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt);
    virtual int do_plan_3th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt);
    virtual int do_plan_5th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt);
    virtual int do_plan_7th(double s0, double se, double v0, double ve, double a0, double ae, double j0, double je, double dt);

private:
    TrajPoint target_ = {};      // 目标点，共享变量，需要锁保护
    TrajPoint pre_target_ = {};  // 上一目标点，共享变量，需要锁保护
    bool is_traj_start = false;  // 标记traj是否启动
    int lost_count_ = 0;
    double planner_interval_ = 0;  // 当前规划器间隔
    double traj_interval_ = 0;     // 外层规划器间隔
    double poly_para_[9] = {};

    std::shared_mutex planner_mutex_;
};

};  // namespace axis